﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using OpenNI;
using NUnit.Framework;

namespace BTKinect.DataType
{
    class KinectPoint3D
    {
        public double X, Y, Z;
        private const string tab = "\t";
        public KinectPoint3D() { }
        public KinectPoint3D(double x, double y, double z)
        {
            X = x; Y = y; Z = z;
        }
        public KinectPoint3D(KinectPoint3D k)
        {
            this.X = k.X;
            this.Y = k.Y;
            this.Z = k.Z;
        }

        public static KinectPoint3D operator -(KinectPoint3D pnt1, KinectPoint3D pntCenter)
        {
            KinectPoint3D tmpP = new KinectPoint3D();
            tmpP.X = pnt1.X - pntCenter.X;
            tmpP.Y = pnt1.Y - pntCenter.Y;
            tmpP.Z = pnt1.Z - pntCenter.Z;
            return tmpP;
        }
        public static KinectPoint3D operator +(KinectPoint3D pnt1, KinectPoint3D pntCenter)
        {
            KinectPoint3D tmpP = new KinectPoint3D();
            tmpP.X = pnt1.X + pntCenter.X;
            tmpP.Y = pnt1.Y + pntCenter.Y;
            tmpP.Z = pnt1.Z + pntCenter.Z;
            return tmpP;
        }
        public static KinectPoint3D operator*(KinectPoint3D pnt1, KinectPoint3D pntCenter)
        {
            KinectPoint3D tmpP = new KinectPoint3D();
            tmpP.X = pnt1.X * pntCenter.X;
            tmpP.Y = pnt1.Y * pntCenter.Y;
            tmpP.Z = pnt1.Z * pntCenter.Z;
            return tmpP;
        }
        public static KinectPoint3D operator /(KinectPoint3D pnt1, KinectPoint3D pntCenter)
        {
            KinectPoint3D tmpP = new KinectPoint3D();
            tmpP.X = pnt1.X / pntCenter.X;
            tmpP.Y = pnt1.Y / pntCenter.Y;
            tmpP.Z = pnt1.Z / pntCenter.Z;
            return tmpP;
        }
        public static KinectPoint3D operator /(KinectPoint3D pnt, double d)
        {
            KinectPoint3D k = new KinectPoint3D();
            k.X= pnt.X / d;
            k.Y = pnt.Y / d;
            k.Z = pnt.Z / d;
            return k;
        }

        public static implicit operator Point3D(KinectPoint3D p)
        {
            Point3D point = new Point3D((float)p.X, (float)p.Y, (float)p.Z);
            return point;
        }
        public static implicit operator KinectPoint3D(Point3D p)
        {
            KinectPoint3D point = new KinectPoint3D(p.X, p.Y, p.Z);
            return point;
        }
        public static implicit operator KinectPoint3D(Vector p)
        {
            return new KinectPoint3D(p[0], p[1], p[2]);
        }
        public static implicit operator Vector(KinectPoint3D p)
        {
            return new Vector(new double[]{p.X,p.Y,p.Z});
        }
        public static implicit operator KinectVector(KinectPoint3D p)
        {
            return new KinectVector(new double[] { p.X, p.Y, p.Z });
        }
        public static implicit operator double[](KinectPoint3D p)
        {
            return new double[] { p.X, p.Y, p.Z };
        }
        public static implicit operator KinectPoint3D(double[] d)
        {
            return new KinectPoint3D(d[0], d[1], d[2]);
        }

        ///// <summary>
        ///// Calculates the distance between two 3D points
        ///// </summary>
        ///// <param name="point1"></param>
        ///// <param name="point2"></param>
        ///// <returns></returns>
        //public float CalculateDistanceBetweenPoint3D(KinectPoint3D point1, KinectPoint3D point2)
        //{
        //    //OLD: temp var
        //    float x, y, z;

        //    //calculate absolute lengths
        //    x = point1.X - point2.X;
        //    y = point1.Y - point2.Y;
        //    z = point1.Z - point2.Z;

        //    //return calculated length 
        //    return (float)Math.Sqrt(((x * x) + (y * y) + (z * z)));
        //}

        public double Distance(KinectPoint3D point2)
        {
            //OLD: temp var
            double x, y, z;

            //calculate absolute lengths
            x = this.X - point2.X;
            y = this.Y - point2.Y;
            z = this.Z - point2.Z;

            //return calculated length 
            return Math.Sqrt(((x * x) + (y * y) + (z * z)));
        }

        public KinectPoint3D Round(int numberOfDecimals)
        {
            return new KinectPoint3D(Math.Round(this.X, numberOfDecimals), Math.Round(this.Y, numberOfDecimals), Math.Round(this.Z, numberOfDecimals));
        }
        public void RoundMe(int numberOfDecimals)
        {
            this.X = Math.Round(this.X, numberOfDecimals);
            this.Y = Math.Round(this.Y, numberOfDecimals);
            this.Z = Math.Round(this.Z, numberOfDecimals);
        }
        public override string ToString()
        {
            return (X.ToString() + tab + Y.ToString() + tab + Z.ToString());
        }
    }
}
